#include "lm3s2965.h"
#include "hw_ints.h"
#include "hw_memmap.h"
#include "hw_types.h"
#include "debug.h"
#include "gpio.h"
#include "interrupt.h"
#include "sysctl.h"
#include "uart.h"
#include "rit128x96x4.h"

#include "systick.h"
#include "URF.h"
#include <stdlib.h>
#include <stdio.h>

unsigned long URF_BASE = UART0_BASE;
unsigned long MUX_BASE = GPIO_PORTB_BASE;
unsigned long MUX_S0 = GPIO_PIN_4;
unsigned long MUX_S1 = GPIO_PIN_5;

volatile char validRead = false;	// is the range reading valid, used after switching sensors
char range[3];			// range value to return
char rangeTemp[3];
volatile char ReadingRange = false;
volatile int counter = 0;



// setup ultrasonic range sensor 
void initURF(void)
{	
	// enable peripherals
	SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

	// Configure the GPIO pin muxing for the UART function.
	GPIOPinConfigure(GPIO_PA0_U0RX);
	GPIOPinConfigure(GPIO_PA1_U0TX);

	// Set GPIO A0 and A1 as UART pins.
	GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

	// configure UART protocol
	UARTConfigSetExpClk(URF_BASE, 7812500, 9600,UART_CONFIG_WLEN_8|UART_CONFIG_STOP_ONE |UART_CONFIG_PAR_NONE);	// 9600 baud, 8 data bits, no parity, one stop bit

	// disable FIFO
	UARTFIFODisable(URF_BASE);

	// enable interrupts
	IntEnable(INT_UART0);
	UARTIntEnable(URF_BASE, UART_INT_RX);    // | UART_INT_RT);


	/*************** set MUX pins ******************/
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	GPIODirModeSet(MUX_BASE, MUX_S0|MUX_S1, GPIO_DIR_MODE_OUT);
	GPIOPadConfigSet(MUX_BASE, MUX_S0|MUX_S1, GPIO_STRENGTH_4MA, GPIO_PIN_TYPE_STD);
	/*************************************************/

	// enable processor interrupts
	IntMasterEnable();

	// enable UART
	UARTEnable(URF_BASE);
}


void displayURFRange(char i){
    printf("URF %d = %s cm\t", i+1, getURFRangeString(i));
}

range_t getURFRange(char i)
{
	switchURF(i);
	// wait until we've gotten a valid read
	while(!validRead){
		// wait

		// TODO: start a timer to get a timeout if we're not reading anything from the URF
	}

	return atoi(range);
}

char* getURFRangeString(char i)
{
	switchURF(i);

	while(!validRead){
		// wait

		// TODO: start a timer to get a timeout if we're not reading anything from the URF
	}
	return range;
}

void UART0_Int_Handler(void){
    unsigned long ulStatus;
	ulStatus = UARTIntStatus(UART0_BASE, true);

	    //
	    // Clear the asserted interrupts.
	    //
	    UARTIntClear(UART0_BASE, ulStatus);

	while(UARTCharsAvail(UART0_BASE))
    {

        if(ReadingRange){
        	rangeTemp[counter] = UARTCharGetNonBlocking(UART0_BASE);
        	counter ++;
        	if (counter == 3){
        		counter = 0;
        		ReadingRange = false;

        		// copy over range
        		int i;
        		for(i = 0; i<3; i++){
        			range[i]=rangeTemp[i];
        		}
        		validRead = true;
        	}
        }else{
        	char value = UARTCharGetNonBlocking(UART0_BASE);
        	if(value =='R'){
        		ReadingRange = true;
        	}
        }

    }

}


void switchURF(char URF_ID)
{
	UARTIntDisable(URF_BASE, UART_INT_RX);
	validRead = false;
	ReadingRange = false;
	counter = false;
	// TODO: MUX lines for controlling which URF to read
	switch(URF_ID)
	{
	case 0:
		GPIOPinWrite(MUX_BASE, MUX_S0, 0x00);
		GPIOPinWrite(MUX_BASE, MUX_S1, 0x00);
		break;
	case 1:
		GPIOPinWrite(MUX_BASE, MUX_S0, 0xFF);
		GPIOPinWrite(MUX_BASE, MUX_S1, 0);
		break;
	case 2:
		GPIOPinWrite(MUX_BASE, MUX_S0, 0);
		GPIOPinWrite(MUX_BASE, MUX_S1, 0xFF);
		break;
	}

	UARTIntClear(UART0_BASE, 0xFF);	// Clear any interrupts
	if (UARTCharsAvail(UART0_BASE)){
		UARTCharGetNonBlocking(UART0_BASE); // do a read to clear uart register
	}

	UARTIntEnable(URF_BASE, UART_INT_RX);	// reenable interrupts
}

